#ifndef ANTI_GYRO_H
#define ANTI_GYRO_H

#include <iostream>
#include <mutex>

#include "../queues/circlequeue.h"
#include "../../../tool/include/types.h"
// #include "interfaces/msg/autoaim_recorder.hpp"


// 对于小陀螺建立一个从自己观测的一个圆弧，运动模型
// 对于变化的量用一阶滞后滤波， 比如陀螺周期
// 对于基本不变的量用平均值

class AntiGyro 
{

public:
    std::recursive_mutex gyro_description_mutex;

    /// 陀螺状态
    GyroDescription gyro_description;

    /// 最近一次装甲板切换的时刻
    int64_t last_armor_switch_clock = 0.0;

    /// 记录车辆装甲板状态，在切换装甲板时不清空，只有在切换车辆的时候才会清空
    CircleQueue<InputOfGyro, 100> car_armors_data;

    /// 记录车辆装甲板状态，在切换装甲板时清空，只有在切换车辆
    CircleQueue<InputOfGyro, 100> same_armor_data;

    // 装甲板数量
    int armor_num = 4;

    // 云台最大角速度设定值，单位度/s
    double gimbal_max_yaw_speed = 90;


    /// 记录装甲板切换的时间戳， 单位ms
    CircleQueue<int64_t, 50> armor_switch_clock_recorder;

    /// 记录左侧切换装甲板数据
    CircleQueue<Target, 5> left_switch_targets;

    /// 记录右侧切换装甲板数据
    CircleQueue<Target, 5> right_switch_targets;

public:
    void update(const InputOfGyro &data);

    void judge_gyro_status_when_armor_switched(double distance);

    /**
    * @brief 判断陀螺描述器是否在时间上已经过期
    * @details 用于在长时间检测不到装甲板的时候及时退出陀螺状态
    */
    bool If_gyro_description_updated(int64_t now); 

    /// 切换车辆时清空
    void clear_if_car_switched();

    /// 切换装甲板
    void clear_if_armor_switched();

    /**
    * @brief 计算小陀螺状态
    * @details 包括陀螺是否有效/陀螺状态/陀螺周期/左右两侧装甲板对应的yaw宽度
    * @details 敌方机器人平面平移运动带来的yaw观测角速度
    * @details 平均z高度/左右两侧装甲板对应的z高度
    */
    void calculate_gyro_info();

    /// 计算打击高速陀螺或者远处陀螺的云台设定值
    void high_or_far_gyro_predict(double v, int64_t now, SendPack &send_pack);

    /**
    * @brief 在高速陀螺模式下选择陀螺中心进行击打
    * 
    */
    void hit_center(double v, int64_t now, SendPack &send_pack);

    /**
        * @brief 早在高速陀螺模式下选择中位数进行击打
        * 
        */
    void hit_median(double v, int64_t now, SendPack &send_pack);

    /**
    * @brief 按照陀螺模型进行击打
    * 
    */
    void hit_accord_gyro_model(double v, int64_t now, SendPack &send_pack);

    /**
    * @brief 优化陀螺模型进行击打   赛季：2023
    *           
    */
    void hit_accord_gyro_model_new(double v, int64_t now, SendPack &send_pack);

    /**
     * @brief 瞄中间自动发弹
     * 
     */
    void aim_center_auto_shoot(double v, int64_t now, SendPack &send_pack); 


    /**
    * @brief 图形化展示当前陀螺计算的一些信息
    * @details 包括陀螺的中心/转速/旋转方向/预测击打点/过去对现在的预测等信息
    * 
    */
    void show_result(cv::Mat &canvas, double v, int64_t now);

    // 
    double get_average_xy_distancce();

    double get_average_z_distancce();

    double get_average_same_armor_z_distancce();   

    void judge_rotate_dir(double yaw_speed, ArmorDir &dir);
};

#endif// ANTI_GYRO_H